//------------------------------------------------------------------------------
//  <copyright file="ImuState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.EV4
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Inertial Measurement Unit implementation for EV4
    /// </summary>
    public class ImuState
    {
        /// <summary>
        /// The right-left acceleration measured in G forces
        /// </summary>
        private double accelerationXInG;

        /// <summary>
        /// The up-down acceleration measured in G forces
        /// </summary>
        private double accelerationYInG;

        /// <summary>
        /// The forwards-backwards acceleration measured in G forces
        /// </summary>
        private double accelerationZInG;
        
        /// <summary>
        /// The remaining number of gyro readings before gyro calibration is complete
        /// </summary>
        private int gyroBiasCalibrationReadingsRemaining;

        /// <summary>
        /// The results of gyro calibration
        /// </summary>
        private Vector3 gyroBias;

        /// <summary>
        /// The rotational velocity on the X axis measured in radians per second
        /// </summary>
        private double gyroXInRadiansPerSecond;

        /// <summary>
        /// The rotational velocity on the Y axis measured in radians per second
        /// </summary>
        private double gyroYInRadiansPerSecond;

        /// <summary>
        /// The rotational velocity on the Z axis measured in radians per second
        /// </summary>
        private double gyroZInRadiansPerSecond;

        /// <summary>
        /// The number of gyro readings that must be made in order to calibrate the gyro bias
        /// </summary>
        private int initialGyroBiasCalibrationReadingCount;
        
        /// <summary>
        /// The interval length between gyro integrations
        /// </summary>
        private double maxGyroIntegrationIntervalInMilliseconds;
        
        /// <summary>
        /// The time of the last gyro integration
        /// </summary>
        private double lastGyroIntegrationInMilliseconds;

        /// <summary>
        /// The previous rotational velocity on the Z axis measured in radians per second
        /// </summary>
        private double lastGyroZInRadiansPerSecond;

        /// <summary>
        /// The cumalative rotational velocity on the Z axis measured in radians per second
        /// </summary>
        private double gyroZRotationSinceStartInRadians;

        /// <summary>
        /// Initializes a new instance of the <see cref="ImuState" /> class.
        /// </summary>
        /// <param name="requiredGyroMeasurementsForCalibration">Number of gyro readings to take before gyro is calibrated</param>
        /// <param name="maxGyroIntegrationIntervalInMilliseconds">The maximum time to allow between gyro integrations</param>
        public ImuState(int requiredGyroMeasurementsForCalibration, double maxGyroIntegrationIntervalInMilliseconds)
        {
            this.initialGyroBiasCalibrationReadingCount = requiredGyroMeasurementsForCalibration;
            this.gyroBiasCalibrationReadingsRemaining = requiredGyroMeasurementsForCalibration;
            this.maxGyroIntegrationIntervalInMilliseconds = maxGyroIntegrationIntervalInMilliseconds;
        }

        /// <summary>
        /// Gets the right-left acceleration measured in G forces
        /// </summary>
        public double AccelerationXInG
        {
            get
            {
                return this.accelerationXInG;
            }
        }

        /// <summary>
        /// Gets the forward-backward acceleration measured in G forces
        /// </summary>
        public double AccelerationYInG
        {
            get
            {
                return this.accelerationYInG;
            }
        }

        /// <summary>
        /// Gets the up-down acceleration measured in G forces
        /// </summary>
        public double AccelerationZInG
        {
            get
            {
                return this.accelerationZInG;
            }
        }

        /// <summary>
        /// Gets the rotational velocity on the X axis measured in radians per second
        /// </summary>
        public double GyroXInRadiansPerSecond
        {
            get
            {
                return this.gyroXInRadiansPerSecond;
            }
        }

        /// <summary>
        /// Gets the rotational velocity on the Y axis measured in radians per second
        /// </summary>
        public double GyroYInRadiansPerSecond
        {
            get
            {
                return this.gyroYInRadiansPerSecond;
            }
        }

        /// <summary>
        /// Gets the rotational velocity on the Z axis measured in radians per second
        /// </summary>
        public double GyroZInRadiansPerSecond
        {
            get
            {
                return this.gyroZInRadiansPerSecond;
            }
        }

        /// <summary>
        /// Gets the accumalated radial rotation around the Z-axes
        /// </summary>
        public double GyroZRotationSinceStartInRadians
        {
            get
            {
                return this.gyroZRotationSinceStartInRadians;
            }
        }

        /// <summary>
        /// Update state of IMU alternate contract.
        /// </summary>
        /// <param name="accelerometer">The latest accelerometer state</param>
        /// <param name="gyroscope">The latest gyroscope state</param>
        public void ImuHeartbeatUpdate(AccelerometerState accelerometer, GyroscopeState gyroscope)
        {
            this.accelerationXInG = (double)accelerometer.AccelerationInG.X;
            this.accelerationYInG = (double)accelerometer.AccelerationInG.Y;
            this.accelerationZInG = (double)accelerometer.AccelerationInG.Z;

            this.gyroXInRadiansPerSecond = -((double)gyroscope.RotationalVelocityInRadiansPerSecond.X) -
                ((this.gyroBiasCalibrationReadingsRemaining > 0) ? 0 : this.gyroBias.X);

            this.gyroYInRadiansPerSecond = -((double)gyroscope.RotationalVelocityInRadiansPerSecond.Y) -
                ((this.gyroBiasCalibrationReadingsRemaining > 0) ? 0 : this.gyroBias.Y);

            this.gyroZInRadiansPerSecond = -((double)gyroscope.RotationalVelocityInRadiansPerSecond.Z) -
                ((this.gyroBiasCalibrationReadingsRemaining > 0) ? 0 : this.gyroBias.Z);

            if (this.gyroBiasCalibrationReadingsRemaining > 0)
            {
                this.gyroBias.X += gyroscope.RotationalVelocityInRadiansPerSecond.X;
                this.gyroBias.Y += gyroscope.RotationalVelocityInRadiansPerSecond.Y;
                this.gyroBias.Z += gyroscope.RotationalVelocityInRadiansPerSecond.Z;

                this.gyroBiasCalibrationReadingsRemaining--;

                if (this.gyroBiasCalibrationReadingsRemaining == 0)
                {
                    this.gyroBias.X /= this.initialGyroBiasCalibrationReadingCount;
                    this.gyroBias.Y /= this.initialGyroBiasCalibrationReadingCount;
                    this.gyroBias.Z /= this.initialGyroBiasCalibrationReadingCount;
                    this.lastGyroZInRadiansPerSecond = this.gyroBias.Y;
                }
            }
            else
            {
                // only do integration if we are not calibrating
                // integrate around Y axis
                double currentTimeInMs = TimeSpan.FromTicks(DateTime.Now.Ticks).TotalMilliseconds;
                if (currentTimeInMs - this.lastGyroIntegrationInMilliseconds < this.maxGyroIntegrationIntervalInMilliseconds)
                {
                    this.gyroZRotationSinceStartInRadians +=
                        ((this.gyroYInRadiansPerSecond + this.lastGyroZInRadiansPerSecond) / 2) *
                        ((currentTimeInMs - this.lastGyroIntegrationInMilliseconds) / 1000.0);
                    this.lastGyroZInRadiansPerSecond = this.gyroZInRadiansPerSecond;
                }

                this.lastGyroIntegrationInMilliseconds = currentTimeInMs;
            }
        }
    }
}
